#include "includes.h"

// ctors
Matrix4x4::Matrix4x4() { 
	
	loadIdentity(); 

}

Matrix4x4::Matrix4x4( float m_[16] ) { 
	
	memcpy( m, m_, 16*sizeof( float ) ); 

}

Matrix4x4::Matrix4x4( const Matrix4x4 & m_ ) { 
	
	for( int c = 0; c < 16; c++ ) m[c] = m_.m[c];

}

// takes args in column major format for ease of reading
Matrix4x4::Matrix4x4( float m0, float m4, float m8,  float m12,
					  float m1, float m5, float m9,  float m13,
					  float m2, float m6, float m10, float m14,
					  float m3, float m7, float m11, float m15 ) {
	
	m[ 0] = m0; m[ 4] = m4; m[ 8] = m8;  m[12] = m12;
	m[ 1] = m1; m[ 5] = m5; m[ 9] = m9;  m[13] = m13;
	m[ 2] = m2; m[ 6] = m6; m[10] = m10; m[14] = m14;
	m[ 3] = m3; m[ 7] = m7; m[11] = m11; m[15] = m15;
	
}

// named ctors
Matrix4x4 Matrix4x4::RotationMatrix( float theta, const Vector3 & axis ) {
	
	Matrix4x4 r;
	r.loadRotate( theta, axis );
	return( r );
	
}

Matrix4x4 Matrix4x4::TranslationMatrix( const Vector3 & v ) {
	
	Matrix4x4 r;
	r.loadTranslate( v );
	return( r );
	
}

Matrix4x4 Matrix4x4::Inverse( const Matrix4x4 & m_ ) {

	Matrix4x4 r( m_ );
	r.invert();
	return( r );

}

// utility
void Matrix4x4::loadIdentity() {
	
	for( int c = 0; c < 16; c++ )
		m[c] = ( c % 5 ) ? 0.0 : 1.0;
	
	
}

void Matrix4x4::loadRotate( double theta, const Vector3 & axis ) {
	
	/*
	From Siggraph.org
	-----------------

	Row1	n1^2 + (1 - n1^2)cosq	   n1n2(1 - cosq) + n3sinq     n1n3(1 - cosq) - n2sinq   0
			
	Row2	n1n2(1 - cosq) - n3sinq    n2^2 + (1 - n2^2)cosq       n2n3(1 - cosq) + n1sinq   0
			  
	Row3	n1n3(1 - cosq) + n2sinq    n2n3(1 - cosq) - n1sinq      n3^2 + (1 - n3^2)cosq    0
				
	Row 4            0 				                0				           0		     1
	*/
	
	double cq = cos( theta * ( 3.1415926536 / 180.0 ) ); // this amount of dps on pi seems to be
	double sq = sin( theta * ( 3.1415926536 / 180.0 ) ); // about the sweet spot
	
	Vector3 na = axis;
	na.normalize();
	
	double n1 = na.x; // n1, n2, n3
	double n2 = na.y;
	double n3 = na.z;
	
	// ROW 1
	m[ 0] = n1 * n1 + ( 1.0 - n1 * n1 ) * cq;
	m[ 4] = n1 * n2 * ( 1.0 - cq ) + n3 * sq;
	m[ 8] = n1 * n3 * ( 1.0 - cq ) - n2 * sq;
	m[12] = 0.0;
	
	// ROW 2
	m[ 1] = n1 * n2 * ( 1.0 - cq ) - n3 * sq;
	m[ 5] = n2 * n2 + ( 1.0 - n2 * n2 ) * cq;
	m[ 9] = n2 * n3 * ( 1.0 - cq ) + n1 * sq;
	m[13] = 0.0;
	
	// ROW 3
	m[ 2] = n1 * n3 * ( 1.0 - cq ) + n2 * sq;
	m[ 6] = n2 * n3 * ( 1.0 - cq ) - n1 * sq;
	m[10] = n3 * n3 + ( 1.0 - n3 * n3 ) * cq;
	m[14] = 0.0;
	
	// ROW 4
	m[ 3] = 0.0;
	m[ 7] = 0.0;
	m[11] = 0.0;
	m[15] = 1.0;
   
	
}

void Matrix4x4::loadTranslate( const Vector3 & v ) {
	
	loadIdentity();
	
	m[12] = v.x;
	m[13] = v.y;
	m[14] = v.z;
	
}

void Matrix4x4::invert() {

	// this is my notation when I worked this out on a piece of paper
	// row-major alphabetic representation
	#define A m[ 0]
	#define E m[ 1]
	#define I m[ 2]
	#define M m[ 3]
	#define B m[ 4]
	#define F m[ 5]
	#define J m[ 6]
	#define N m[ 7]
	#define C m[ 8]
	#define G m[ 9]
	#define K m[10]
	#define O m[11]
	#define D m[12]
	#define H m[13]
	#define L m[14]
	#define P m[15]

	// 2x2 matrix determinants we will need
	// all other determinants are calculated
	// by expanding along columns or rows
	float d1  = K*P-L*O;
	float d2  = G*P-O*H;
	float d3  = G*L-H*K;
	float d4  = C*P-D*O;
	float d5  = C*L-D*K;
	float d6  = C*H-D*G;
	float d7  = E*J-F*I;
	float d8  = E*N-F*M;
	float d9  = I*N-J*M;
	float d10 = A*J-B*I;
	float d11 = A*N-B*M;
	float d12 = A*F-B*E;

	// temporary matrix
	float n[16];

	// start by calculating cofactors for first column
	// when devided by det(M) these form the first row of M^-1
	n[ 0] =  ( F * d1 - J * d2 + N * d3 );
	n[ 4] = -( B * d1 - J * d4 + N * d5 );
	n[ 8] =  ( B * d2 - F * d4 + N * d6 );
	n[12] = -( B * d3 - F * d5 + J * d6 );

	// calulate det(M) by cofactor expansion along the first column
	float dm = A * n[0] + E * n[4] + I * n[8] + M * n[12];

	if( dm == 0.0 ) {

		cout << "det(M) = 0, matrix is not invertible\n";
		return;

	}

	// divide first row by det(M) to give first row of inverse
	n[ 0] /= dm;
	n[ 4] /= dm;
	n[ 8] /= dm;
	n[12] /= dm;

	// calculate the terms for the rest of M^-1, by:
	//
	// (M^-1)ij = (-1)^(i+j) * det( m(ji)(M) ) / det( M )
	//
	// where m(ji)(M) represents a submatrix of M with the j-th column and i-th row
	// removed (i,e a 3x3 matrix)

	// second row (expanding 3x3 matrices along the first column)
	n[ 1] = -( E * d1 - I * d2 + M * d3 ) / dm;
	n[ 5] =  ( A * d1 - I * d4 + M * d5 ) / dm;
	n[ 9] = -( A * d2 - E * d4 + M * d6 ) / dm;
	n[13] =  ( A * d3 - E * d5 + I * d6 ) / dm;

	// third row (expanding along the last column of each 3x3...)
	n[ 2] =  ( H * d9 - L * d8  + P * d7  ) / dm;
	n[ 6] = -( D * d9 - L * d11 + P * d10 ) / dm;
	n[10] =  ( D * d8 - H * d11 + P * d12 ) / dm;
	n[14] = -( D * d7 - H * d10 + L * d12 ) / dm;

	// fourth row = same
	n[ 3] = -( G * d9 - K * d8  + O * d7  ) / dm;
	n[ 7] =  ( C * d9 - K * d11 + O * d10 ) / dm;
	n[11] = -( C * d8 - G * d11 + O * d12 ) / dm;
	n[15] =  ( C * d7 - G * d10 + K * d12 ) / dm;

	memcpy( m, n, 16 * sizeof( float ) );

	#undef A
	#undef E
	#undef I
	#undef M
	#undef B
	#undef F
	#undef J
	#undef N
	#undef C
	#undef G
	#undef K
	#undef O
	#undef D
	#undef H
	#undef L
	#undef P

}

// operators
Vector3 Matrix4x4::operator * ( const Vector3 & v ) const {
	
	return( Vector3( v.x * m[ 0] + v.y * m[ 4] + v.z * m[ 8],
		v.x * m[ 1] + v.y * m[ 5] + v.z * m[ 9],
		v.x * m[ 2] + v.y * m[ 6] + v.z * m[10] ));
	
	
}

Matrix4x4 Matrix4x4::operator * ( const Matrix4x4 & m_ ) const {
	
	float r[16];
	
	r[ 0] = m[ 0] * m_.m[ 0] + m[ 4] * m_.m[ 1] + m[ 8] * m_.m[ 2] + m[12] * m_.m[ 3];
	r[ 1] = m[ 1] * m_.m[ 0] + m[ 5] * m_.m[ 1] + m[ 9] * m_.m[ 2] + m[13] * m_.m[ 3];
	r[ 2] = m[ 2] * m_.m[ 0] + m[ 6] * m_.m[ 1] + m[10] * m_.m[ 2] + m[14] * m_.m[ 3];
	r[ 3] = m[ 3] * m_.m[ 0] + m[ 7] * m_.m[ 1] + m[11] * m_.m[ 2] + m[15] * m_.m[ 3];
	
	r[ 4] = m[ 0] * m_.m[ 4] + m[ 4] * m_.m[ 5] + m[ 8] * m_.m[ 6] + m[12] * m_.m[ 7];
	r[ 5] = m[ 1] * m_.m[ 4] + m[ 5] * m_.m[ 5] + m[ 9] * m_.m[ 6] + m[13] * m_.m[ 7];
	r[ 6] = m[ 2] * m_.m[ 4] + m[ 6] * m_.m[ 5] + m[10] * m_.m[ 6] + m[14] * m_.m[ 7];
	r[ 7] = m[ 3] * m_.m[ 4] + m[ 7] * m_.m[ 5] + m[11] * m_.m[ 6] + m[15] * m_.m[ 7];
	
	r[ 8] = m[ 0] * m_.m[ 8] + m[ 4] * m_.m[ 9] + m[ 8] * m_.m[10] + m[12] * m_.m[11];
	r[ 9] = m[ 1] * m_.m[ 8] + m[ 5] * m_.m[ 9] + m[ 9] * m_.m[10] + m[13] * m_.m[11];
	r[10] = m[ 2] * m_.m[ 8] + m[ 6] * m_.m[ 9] + m[10] * m_.m[10] + m[14] * m_.m[11];
	r[11] = m[ 3] * m_.m[ 8] + m[ 7] * m_.m[ 9] + m[11] * m_.m[10] + m[15] * m_.m[11];
	
	r[12] = m[ 0] * m_.m[12] + m[ 4] * m_.m[13] + m[ 8] * m_.m[14] + m[12] * m_.m[15];
	r[13] = m[ 1] * m_.m[12] + m[ 5] * m_.m[13] + m[ 9] * m_.m[14] + m[13] * m_.m[15];
	r[14] = m[ 2] * m_.m[12] + m[ 6] * m_.m[13] + m[10] * m_.m[14] + m[14] * m_.m[15];
	r[15] = m[ 3] * m_.m[12] + m[ 7] * m_.m[13] + m[11] * m_.m[14] + m[15] * m_.m[15];
	
	return( Matrix4x4( r ) );
	
}

Matrix4x4 Matrix4x4::operator *= ( const Matrix4x4 & m_ ) {
	
	float r[16];
	
	r[ 0] = m[ 0] * m_.m[ 0] + m[ 4] * m_.m[ 1] + m[ 8] * m_.m[ 2] + m[12] * m_.m[ 3];
	r[ 1] = m[ 1] * m_.m[ 0] + m[ 5] * m_.m[ 1] + m[ 9] * m_.m[ 2] + m[13] * m_.m[ 3];
	r[ 2] = m[ 2] * m_.m[ 0] + m[ 6] * m_.m[ 1] + m[10] * m_.m[ 2] + m[14] * m_.m[ 3];
	r[ 3] = m[ 3] * m_.m[ 0] + m[ 7] * m_.m[ 1] + m[11] * m_.m[ 2] + m[15] * m_.m[ 3];
	
	r[ 4] = m[ 0] * m_.m[ 4] + m[ 4] * m_.m[ 5] + m[ 8] * m_.m[ 6] + m[12] * m_.m[ 7];
	r[ 5] = m[ 1] * m_.m[ 4] + m[ 5] * m_.m[ 5] + m[ 9] * m_.m[ 6] + m[13] * m_.m[ 7];
	r[ 6] = m[ 2] * m_.m[ 4] + m[ 6] * m_.m[ 5] + m[10] * m_.m[ 6] + m[14] * m_.m[ 7];
	r[ 7] = m[ 3] * m_.m[ 4] + m[ 7] * m_.m[ 5] + m[11] * m_.m[ 6] + m[15] * m_.m[ 7];
	
	r[ 8] = m[ 0] * m_.m[ 8] + m[ 4] * m_.m[ 9] + m[ 8] * m_.m[10] + m[12] * m_.m[11];
	r[ 9] = m[ 1] * m_.m[ 8] + m[ 5] * m_.m[ 9] + m[ 9] * m_.m[10] + m[13] * m_.m[11];
	r[10] = m[ 2] * m_.m[ 8] + m[ 6] * m_.m[ 9] + m[10] * m_.m[10] + m[14] * m_.m[11];
	r[11] = m[ 3] * m_.m[ 8] + m[ 7] * m_.m[ 9] + m[11] * m_.m[10] + m[15] * m_.m[11];
	
	r[12] = m[ 0] * m_.m[12] + m[ 4] * m_.m[13] + m[ 8] * m_.m[14] + m[12] * m_.m[15];
	r[13] = m[ 1] * m_.m[12] + m[ 5] * m_.m[13] + m[ 9] * m_.m[14] + m[13] * m_.m[15];
	r[14] = m[ 2] * m_.m[12] + m[ 6] * m_.m[13] + m[10] * m_.m[14] + m[14] * m_.m[15];
	r[15] = m[ 3] * m_.m[12] + m[ 7] * m_.m[13] + m[11] * m_.m[14] + m[15] * m_.m[15];
	
	memcpy( m, r, 16 * sizeof( float ) );
	
	return( *this );
	
}

ostream & operator << ( ostream & o, const Matrix4x4 & m_ ) {

		float n[16]; // temp storage
		memcpy( n, m_.m, 16 * sizeof( float ) );

		for( int c = 0; c < 16; c++ )
			if( fabs( n[c] ) < 0.000001 ) n[c] = 0.0;
	
		o << "[" << n[ 0] << "\t" << n[ 4] << "\t" << n[ 8] << "\t" << n[12] << "]\n";
		o << "|" << n[ 1] << "\t" << n[ 5] << "\t" << n[ 9] << "\t" << n[13] << "|\n";
		o << "|" << n[ 2] << "\t" << n[ 6] << "\t" << n[10] << "\t" << n[14] << "|\n";
		o << "[" << n[ 3] << "\t" << n[ 7] << "\t" << n[11] << "\t" << n[15] << "]\n";

		return( o );
	
}
